/*
 *  guiControl.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2008-03-18.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "guiControl.h"

#define NUMCONTROLVALS  8
#define NUMMATRIXVALS   44

extern double calibration;

void guiIO(Server bandpGUILink, CvKalman **kalman_ball, SetPoint *setpoint, ImageAnalysisParams *params, CvMat **K_dlqr, bool *startStop, int *displayType, int *missingCount, int *ballGone, int *keyHold) {
    
    float F[NUMMATRIXVALS+NUMCONTROLVALS]; //Create array that will receive matrix data from Java GUI
    
    int count=0;
    do{
        if(!*startStop){
            cvWaitKey(1000);
            *missingCount=1000;
            *ballGone=1;
            if (count < 1)
                printf("Table stopped. Press go to continue.\n");
        }
//                printf("Reading matrices from TCP port...\n");
        int numReadFromSocket = bandpGUILink.recv_floats( F, NUMMATRIXVALS+NUMCONTROLVALS);
        if (numReadFromSocket == NUMMATRIXVALS+NUMCONTROLVALS){
//                printf("Writing matrices from TCP port...\n");
            memcpy( (*kalman_ball)->process_noise_cov->data.fl, F, sizeof(float)*16 );
            memcpy( (*kalman_ball)->measurement_noise_cov->data.fl, F+16, sizeof(float)*4 );
            memcpy( (*kalman_ball)->error_cov_post->data.fl, F+20, sizeof(float)*16 );
            memcpy( (*K_dlqr)->data.fl, F+36, sizeof(float)*8 );
            
            setpoint->ballSpeedR       = (double)F[NUMMATRIXVALS+0]/1000;
            setpoint->pathNum         = (int)F[NUMMATRIXVALS+1];
            setpoint->pathRadius      = (double)F[NUMMATRIXVALS+2];
            params->cannyThreshHigh = (int)F[NUMMATRIXVALS+3];
            params->cannyThreshLow  = (int)F[NUMMATRIXVALS+4];
            params->binaryThresh    = (int)F[NUMMATRIXVALS+5];
            *startStop		 = (bool)F[NUMMATRIXVALS+6];
            *displayType		 = (int)F[NUMMATRIXVALS+7];
            *keyHold='0';
            
//            std::cout << "\nballSpeed is \n" << setpoint->ballSpeed;
//            std::cout << "\nballSpeedR is \n" << setpoint->ballSpeedR;
//            std::cout << "\npathNum is \n" << setpoint->pathNum;
//            std::cout << "\npathRadius is \n" << setpoint->pathRadius << std::endl;
            std::cout << "\nK_dlqr is \n";
            printCvArr(*K_dlqr);
            std::cout << "\nQk is \n";
            printCvArr((*kalman_ball)->process_noise_cov);
            std::cout << "\nRk is \n";
            printCvArr((*kalman_ball)->measurement_noise_cov);
//            std::cout << "\nPk is \n";
//            printCvArr((*kalman_ball)->error_cov_post);
        }
        count++;
    }while(!*startStop);
    
    return;
}

